/*
 * BallStrategy.cpp
 *
 */

#include "../../include/vision/BallStrategy.h"
#include "../../include/Config.h"

namespace objpreproc {

BallStrategy::BallStrategy() {
	_erosion_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE,
			cv::Size(2 * BALL_EROSION_SIZE + 1, 2 * BALL_EROSION_SIZE + 1),
			cv::Point(BALL_EROSION_SIZE, BALL_EROSION_SIZE));

	_dilatation_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE,
			cv::Size(2 * BALL_DILATATION_SIZE + 1, 2 * BALL_DILATATION_SIZE + 1),
			cv::Point(BALL_DILATATION_SIZE, BALL_DILATATION_SIZE));

	_threshold_values_min_up = new cv::Scalar(BALL_H_MIN_UP_0, BALL_S_MIN_UP_1,
			BALL_V_MIN_UP_2, 0);

	_threshold_values_max_up = new cv::Scalar(BALL_H_MAX_UP_0, BALL_S_MAX_UP_1,
			BALL_V_MAX_UP_2, 0);

	_threshold_values_min_low = new cv::Scalar(BALL_H_MIN_LOW_0, BALL_S_MIN_LOW_1,
				BALL_V_MIN_LOW_2, 0);

	_threshold_values_max_low = new cv::Scalar(BALL_H_MAX_LOW_0, BALL_S_MAX_LOW_1,
				BALL_V_MAX_LOW_2, 0);
	_up_thresh = new cv::Mat(cv::Size(FRAME_WIDTH, FRAME_HEIGHT), CV_8UC1);
	_low_thresh = new cv::Mat(cv::Size(FRAME_WIDTH, FRAME_HEIGHT), CV_8UC1);

	_detection_params = new cv::SimpleBlobDetector::Params();
	_detection_params->filterByArea = true;
	_detection_params->filterByColor = true;
	_detection_params->minDistBetweenBlobs = 100;
	_detection_params->minArea = BALL_MIN_AREA;
	_detection_params->maxArea = BALL_MAX_AREA;
	_detection_params->blobColor = BALL_BLOB_COLOR;
	_simple_blob_detector = new cv::SimpleBlobDetector(*_detection_params);
}

BallStrategy::~BallStrategy() {
	_erosion_kernel.release();
	_dilatation_kernel.release();
	delete _up_thresh;
	delete _low_thresh;
	delete _threshold_values_min_up;
	delete _threshold_values_max_up;
	delete _threshold_values_min_low;
	delete _threshold_values_max_low;
	delete _detection_params;
	delete _simple_blob_detector;

	// Prevenzione del dangling reference.
	_threshold_values_min_up = NULL;
	_threshold_values_max_up = NULL;
	_threshold_values_min_low = NULL;
	_threshold_values_max_low = NULL;
	_detection_params = NULL;
	_simple_blob_detector = NULL;
}

void BallStrategy::doThreshold(const cv::Mat &src, cv::Mat &dest) {
	assert(!src.empty());
	assert(!dest.empty());
	assert(src.size == dest.size);

	cv::inRange(src, *_threshold_values_min_up, *_threshold_values_max_up, *_up_thresh);
	cv::inRange(src, *_threshold_values_min_low, *_threshold_values_max_low, *_low_thresh);
	cv::bitwise_or(*_up_thresh, *_low_thresh, dest);
}

void BallStrategy::doErosion(const cv::Mat &src, cv::Mat &dest) {
	assert(!src.empty());
	assert(!dest.empty());
	assert(src.size == dest.size);
	assert(src.channels() == dest.channels());

	cv::erode(src, dest, _erosion_kernel);
}

void BallStrategy::doDilatation(const cv::Mat &src, cv::Mat &dest) {
	assert(!src.empty());
	assert(!dest.empty());
	assert(src.size == dest.size);
	assert(src.channels() == dest.channels());

	cv::dilate(src, dest, _dilatation_kernel);
}

void BallStrategy::doBlobDetection(const cv::Mat &image, std::vector<cv::KeyPoint> &keypoints, const cv::Mat &mask) {
	assert(!image.empty());

	_simple_blob_detector->detect(image, keypoints, mask);
}

void BallStrategy::doPOIExtraction(const cv::Mat &depth, std::vector<cv::KeyPoint> &keypoints, std::vector<poi::POI*> &pois) {
	assert(!depth.empty());

	for(unsigned int i = 0; i < keypoints.size(); i++) {
		pois.push_back(utils::Utils::KeyPoint2POIfromDepthMat(keypoints[i], BALL, depth));
	}
}

} /* namespace objpreproc */
